/*
 * SimpleStop.cpp
 *
 *  Created on: Nov 14, 2013
 *      Author: ofri
 */

#include "SimpleStop.h"

SimpleStop::SimpleStop() {
	// TODO NO NEED IT HERE because it stops immediately
	int queue_depth = 10;


	// void (*func_pointer)(const sensor_msgs::LaserScan::ConstPtr&);//=frontLaserHandler;
	// func_pointer =(void(*)(const sensor_msgs::LaserScan::ConstPtr&))&SimpleStop::frontLaserHandler;

	node_handler.subscribe("/Module_Sensors_Node/Front_Laser", 	queue_depth, (void(*)(const sensor_msgs::LaserScan::ConstPtr&))&SimpleStop::frontLaserHandler);

	// node_handler.subscribe("/Module_Sensors_Node/Back_Laser",  	queue_depth, backLaserHandler);
	// node_handler.subscribe("/Module_Sensors_Node/Back_US",  	queue_depth, backUSHandler);

	// node_handler.subscribe("/Module_Sensors_Node/Right_Laser", 	queue_depth, rightLaserHandler);
	// node_handler.subscribe("/Module_Sensors_Node/Right_US", 	queue_depth, rightUSHandler);

	// node_handler.subscribe("/Module_Sensors_Node/Left_Laser",  	queue_depth, leftLaserHandler);
	// node_handler.subscribe("/Module_Sensors_Node/Left_US",  	queue_depth, SimpleStop::leftUSHandler);

}

void SimpleStop::act(Driver& driver) {

	// stop the robot
	driver.stop();
}

SimpleStop::~SimpleStop() {
	// TODO Auto-generated destructor stub
}

